
#include <math.h>
#include <stdlib.h>
#include "Vectores.h"
#include "Platform.h"

bool pointIntRec(const float c[3],const Platform* p){
  if (c[0] >= p->position[0] &&
      c[0] <= p->position[0]+p->width &&
      c[1] >= p->position[1] &&
      c[1] <= p->position[1]+p->height)
    return true;
  return false;

}

void dotVector(float A[3], float B[3], float C[3]){
  C[0] = A[1]*B[2] - A[2]*B[1];
  C[1] = -(A[0]*B[2] - A[2]*B[0]);
  C[2] = A[0]*B[1] - A[1]*B[0];
}

bool pointIntTriangle(
    const float P[3],
    const float A[3],
    const float B[3],
    const float C[3])
{
  float v0[3]={0}, v1[3]={0}, v2[3]={0};
  float dot00[3]={0}, dot01[3]={0}, dot02[3]={0}, dot11[3]={0}, dot12[3]={0};

  restarVectores(C, A, v0);
  restarVectores(B, A, v1);
  restarVectores(P, A, v2);

  dotVector(v0, v0, dot00);
  dotVector(v0, v1, dot01);
  dotVector(v0, v2, dot02);
  dotVector(v1, v1, dot11);
  dotVector(v1, v2, dot12);

  float invDenom = 1.0f / (multVectores(dot00, dot11) - multVectores(dot01, dot01));
  float u = ((multVectores(dot11, dot02) - multVectores(dot01, dot12)))*invDenom;
  float v = ((multVectores(dot00, dot12) - multVectores(dot01, dot02)))*invDenom;

  if(u>0 && v>0 && u+v<1)
    return true;
  else
    return false;
}


void restarVectores(float const a[],float const b[],float c[]){
  c[0] = a[0] - b[0];
  c[1] = a[1] - b[1];
//  c[2] = a[2] - b[2];
}

void sumarVectores(float const a[],float const b[],float c[]){
  c[0] = a[0] + b[0];
  c[1] = a[1] + b[1];
//  c[2] = a[2] + b[2];
}

float multVectores(float a[],float b[]){
  return a[0] * b[0] + a[1] * b[1]; // + a[2] * b[2];
}

void normalize(float a[]){
  float len = length(a);
  a[0] /= len;
  a[1] /= len;
//  a[2] /= len;
}

void normalize2(float a[], float b[]){
  float len = length(a);
  b[0] = a[0] / len;
  b[1] = a[1] / len;
//  b[2] = a[2] / len;
}

float length(float const a[]){
  // return sqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);
  return sqrt(a[0]*a[0]+a[1]*a[1]);
}

void multVectorScalar(float a[],float ms){
  a[0] = a[0]*ms;
  a[1] = a[1]*ms;
//  a[2] = a[2]*ms;
}

void divVectorScalar(float a[],float ms){
  a[0] = a[0]/ms;
  a[1] = a[1]/ms;
//  a[2] = a[2]/ms;
}

void multVectorScalar2(float a[],float ms,float b[]){
  b[0] = a[0]*ms;
  b[1] = a[1]*ms;
//  b[2] = a[2]*ms;
}

bool isZero(float a[]){
  return a[0] == 0 && a[1] == 0; // && a[2] == 0;
}

float* asVector(float o){
  float* ret = (float*) malloc(sizeof(float)*3);
  ret[0] = sin(o);
  ret[1] = cos(o);
  ret[2] = 0;
  return ret;
}


/// Clase vectores
Vector3d::Vector3d(): coor(vector<float>(3,0)) { }

Vector3d::Vector3d(float x, float y, float z): coor(vector<float>(3,0)) {
    this->coor[0] = x;
    this->coor[1] = y;
    this->coor[2] = z;
}

Vector3d::Vector3d(float value): coor(vector<float>(3,0)) {
    this->coor[0] = sin(value);
    this->coor[1] = cos(value);
    this->coor[2] = 0;
}

Vector3d Vector3d::operator+(const Vector3d &v) const{
    return Vector3d(
            this->coor[0] + v.coor[0],
            this->coor[1] + v.coor[1],
            this->coor[2] + v.coor[2]);
}

Vector3d Vector3d::operator-(const Vector3d &v) const{
    return Vector3d(
            this->coor[0] - v.coor[0],
            this->coor[1] - v.coor[1],
            this->coor[2] - v.coor[2]);
}

Vector3d Vector3d::operator/(const float value) const{
    return Vector3d(
            this->coor[0] / value,
            this->coor[1] / value,
            this->coor[2] / value );
}

Vector3d Vector3d::operator*(const float value) const{
    return Vector3d(
            this->coor[0] * value,
            this->coor[1] * value,
            this->coor[2] * value );
}

Vector3d Vector3d::operator~() const{
    return (*this) / this->length();
}

float Vector3d::length() const {
  return sqrt(
          this->coor[0] * this->coor[0] +
          this->coor[1] * this->coor[1] +
          this->coor[2] * this->coor[2] );
}

Vector3d & Vector3d::normalize(){
    float length = this->length();
    this->coor[0] /= length;
    this->coor[1] /= length;
    this->coor[2] /= length;
    return *this;
}

